#include <Servo.h>

#define _ROBOT_VERSION_ "0.5"
#define _ROBOT_NAME_ "Spidy"

/****************************/
/* servo parts              */
/****************************/
#define I_LEFT_FRONT_COXA 0
#define I_LEFT_FRONT_FEMUR 1
#define I_LEFT_FRONT_TIBIA 2
#define I_LEFT_MIDDLE_COXA 3
#define I_LEFT_MIDDLE_FEMUR 4
#define I_LEFT_MIDDLE_TIBIA 5
#define I_LEFT_BACK_COXA 6
#define I_LEFT_BACK_FEMUR 7
#define I_LEFT_BACK_TIBIA 8
#define I_RIGHT_FRONT_COXA 9
#define I_RIGHT_FRONT_FEMUR 10
#define I_RIGHT_FRONT_TIBIA 11
#define I_RIGHT_MIDDLE_COXA 12
#define I_RIGHT_MIDDLE_FEMUR 13
#define I_RIGHT_MIDDLE_TIBIA 14
#define I_RIGHT_BACK_COXA 15
#define I_RIGHT_BACK_FEMUR 16
#define I_RIGHT_BACK_TIBIA 17

// define 
#define BT_BAUD 57600

#define DEL_CONNECT 13

#define N_SERVOS 18
Servo servos[N_SERVOS];
const int servo_ind[N_SERVOS] = {
  I_LEFT_FRONT_COXA, I_LEFT_FRONT_FEMUR, I_LEFT_FRONT_TIBIA,
  I_LEFT_MIDDLE_COXA, I_LEFT_MIDDLE_FEMUR, I_LEFT_MIDDLE_TIBIA,
  I_LEFT_BACK_COXA, I_LEFT_BACK_FEMUR, I_LEFT_BACK_TIBIA,

  I_RIGHT_FRONT_COXA, I_RIGHT_FRONT_FEMUR, I_RIGHT_FRONT_TIBIA,
  I_RIGHT_MIDDLE_COXA, I_RIGHT_MIDDLE_FEMUR, I_RIGHT_MIDDLE_TIBIA,
  I_RIGHT_BACK_COXA, I_RIGHT_BACK_FEMUR, I_RIGHT_BACK_TIBIA,
};
const int servo_pin[N_SERVOS] = {
  35, 36, 37,
  27, 28, 29,
  24, 25, 26,

  38, 39, 40,
  46, 47, 48,
  49, 50, 51
};
const int servo_cal[3][N_SERVOS] = {
  { // low 0°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
  ,
  { // medium 90°
    25,  0,  35,
    65,  -45,  45,
    45,  10,  35,

    50,  55,  -25,
    -20,  55,  -25,
    -150,  25,  0
  }
  ,
  { // high 180°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
};
int servo_pos[N_SERVOS] = {
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),

  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
};
boolean servo_update[N_SERVOS] = {
  true,  true,  true,
  true,  true,  true,
  true,  true,  true,

  true,  true,  true,
  true,  true,  true,
  true,  true,  true,
};

boolean g_isReady = false;
boolean g_isConnected = false;
unsigned long g_time=0;
boolean g_isDebug = false;
void setup()
{
  if(Serial)
  {
    Serial.begin(57600); // standard output ^^
  
    Serial.print("Robot '");
    Serial.print(_ROBOT_NAME_);
    Serial.println("' by NooTe");
    Serial.print("version ");
    Serial.println(_ROBOT_VERSION_);
    Serial.println("");
  
    Serial.print("Initialize bluetooth baud as ");
    Serial.println(BT_BAUD);
  }
  Serial1.begin(BT_BAUD); // BT serial

  if(Serial)
  Serial.println("Initialize servomotors...");

  init_servos();
  set_pack_position();
  update_servos();
  delay(2500);

  if(Serial)
    Serial.println("Wait commands...");
  g_isReady = true;
  
  digitalWrite(DEL_CONNECT, HIGH);
  delay(100); 
  digitalWrite(DEL_CONNECT, LOW);  
  delay(100); 
  digitalWrite(DEL_CONNECT, HIGH);
  delay(100); 
  digitalWrite(DEL_CONNECT, LOW);  
  
  g_time = millis();
}

#define TICK_MESURE (1000/60)
void loop()
{
  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  // got bluetooth command ?
  check_commands();

  // update servo positions
  update_servos();

  // send tick
  if(g_isConnected)
  {
    static unsigned long tick_release = 0;
    tick_release += slicetime;
    if(tick_release>TICK_MESURE) // 60 fps
    {
      send_tick(tick_release);
      tick_release-=TICK_MESURE;
    }
  }
}



